#include "Optimizer.h"

#include <complex>

#include <Eigen/StdVector>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

#include "Thirdparty/g2o/g2o/core/sparse_block_matrix.h"
#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
#include "G2oTypes.h"
#include "Converter.h"

#include <mutex>

#include <thread>

namespace msf
{

/**************************************以下为单帧优化**************************************************************/

/**
 * @brief 使用局部关键帧序列信息和IMU信息，优化当前帧位姿
 *
 * 可分为以下几个步骤：
 * // Step 1：创建g2o优化器，初始化顶点和边
 * // Step 2：启动多轮优化，剔除外点
 * // Step 3：更新当前帧位姿、速度、IMU偏置
 * // Step 4：记录当前帧的优化状态，包括参数信息和边缘化后的海森矩阵
 *
 * @param[in] pFrame 当前帧，也是待优化的帧
 * @param[in] bRecInit 调用这个函数的位置并没有传这个参数，因此它的值默认为false
 * @return int 返回优化后的内点数
 */
int Optimizer::PoseInertialOptimization(PoseFrame *pFrame, bool bRecInit)
{
      // Step 1.获取局部关键帧序列
      KeyPoseFrame *pKPF = pFrame->mpLastKeyPoseFrame;
      Map *pCurrentMap = pKPF->GetMap();

      int maxOpt = 3; // 最大优化关键帧数目
      int opt_it = 5; // 每次优化的迭代次数

      // 预计待优化的关键帧数，min函数是为了控制优化关键帧的数量
      const int Nd = std::min((int)pCurrentMap->KeyPoseFramesInMap() - 2, maxOpt);
      const unsigned long maxKPFid = pKPF->mnId;

      vector<KeyPoseFrame *> vpOptimizableKPFs;
      long unsigned int currentPFid;

      vpOptimizableKPFs.reserve(Nd);
      vpOptimizableKPFs.push_back(pKPF);
      //pKPF->mnBALocalForKPF = pKPF->mnId;

      for (int i = 1; i < Nd; i++)
      {
            if (vpOptimizableKPFs.back()->mPrevKPF)
            {
                  vpOptimizableKPFs.push_back(vpOptimizableKPFs.back()->mPrevKPF); // current -> previous
                  //vpOptimizableKPFs.back()->mnBALocalForKPF = pKPF->mnId;
            }
            else
                  break;
      }

      int N = vpOptimizableKPFs.size();

      // Step 2.设置优化器流程
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolverX::LinearSolverType *linearSolver;

      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();

      g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);

      g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      solver->setUserLambdaInit(1e-5);
      optimizer.setAlgorithm(solver);
      optimizer.setVerbose(false);

      // Step 3.设置关键帧与偏置节点
      // 设置KeyPoseFrame节点
      KeyPoseFrame *pIncKPF;  // vpOptimizableKPFs中最后一个id符合要求的关键帧
      for(size_t i=0;i<N;i++)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i];
            if (pKPFi->mnId > maxKPFid)
                  continue;
            VertexPose *VP = new VertexPose(pKPFi);
            VP->setId(pKPFi->mnId);
            VP->setFixed(false);
            pIncKPF = pKPFi;
            optimizer.addVertex(VP);

            if (pKPFi->bImu)
            {
                  VertexVelocity *VV = new VertexVelocity(pKPFi);
                  VV->setId((maxKPFid+1) + 3 * (pKPFi->mnId) + 1);
                  VV->setFixed(false);
                  optimizer.addVertex(VV);

                  VertexGyroBias *VG = new VertexGyroBias(pKPFi);
                  VG->setId((maxKPFid+1) + 3 * (pKPFi->mnId) + 2);
                  VG->setFixed(false);
                  optimizer.addVertex(VG);

                  VertexAccBias *VA = new VertexAccBias(pKPFi);
                  VA->setId((maxKPFid+1) + 3 * (pKPFi->mnId) + 3);
                  VA->setFixed(false);
                  optimizer.addVertex(VA);

                  if(i == 0)
                  {
                        // 设置CurrentPoseFrame节点
                        // 当前帧的位姿，旋转+平移，6-dim
                        // Problem10 : addVertex: FATAL, a vertex with ID 3 has already been registered with this graph
                        VertexPose *VP = new VertexPose(pFrame);
                        VP->setId(maxKPFid + 1);
                        VP->setFixed(false);
                        optimizer.addVertex(VP);

                        // 当前帧的速度，3-dim
                        VertexVelocity *VV = new VertexVelocity(pFrame);
                        VV->setId((maxKPFid+1) + 3 * (maxKPFid + 1) + 1);
                        VV->setFixed(false);
                        optimizer.addVertex(VV);

                        // 当前帧的陀螺仪偏置，3-dim
                        VertexGyroBias *VG = new VertexGyroBias(pFrame);
                        VG->setId((maxKPFid+1) + 3 * (maxKPFid + 1) + 2);
                        VG->setFixed(false);
                        optimizer.addVertex(VG);

                        // 当前帧的加速度偏置，3-dim
                        VertexAccBias *VA = new VertexAccBias(pFrame);
                        VA->setId((maxKPFid+1) + 3 * (maxKPFid + 1) + 3);
                        VA->setFixed(false);
                        optimizer.addVertex(VA);
                  }                  
            }
      }

      vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);
      vector<EdgeGyroRW *> vegr(N, (EdgeGyroRW *)NULL);
      vector<EdgeAccRW *> vear(N, (EdgeAccRW *)NULL);
      // Step 4.添加IMU惯性边、角速度/加速度随即游走残差边
      for(size_t i=0;i<=N-2;i++)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i]; // current -> previous
            // 必须有对应的上一个关键帧
            if (!pKPFi->mPrevKPF)
            {
                  std::cout << "Optimizer::PoseInertialOptimization : NOT INERTIAL LINK TO PREVIOUS FRAME!" << std::endl;
                  continue;
            }

            // KPF
            if (pKPFi->mPrevKPF && pKPFi->mnId <= maxKPFid)
            {
                  if (pKPFi->isBad() || pKPFi->mPrevKPF->mnId > maxKPFid)
                        continue;
                  // 这两个都必须为初始化后的关键帧
                  if (pKPFi->bImu && pKPFi->mPrevKPF->bImu)
                  {
                        // 根据上一帧的偏置设定当前帧的新偏置
                        pKPFi->mpImuPreintegrated->SetNewBias(pKPFi->mPrevKPF->GetImuBias());
                        // 提取节点
                        g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKPFi->mPrevKPF->mnId);
                        g2o::HyperGraph::Vertex *VV1 = optimizer.vertex((maxKPFid+1) + 3 * (pKPFi->mPrevKPF->mnId) + 1);
                        g2o::HyperGraph::Vertex *VG1 = optimizer.vertex((maxKPFid+1) + 3 * (pKPFi->mPrevKPF->mnId) + 2);
                        g2o::HyperGraph::Vertex *VA1 = optimizer.vertex((maxKPFid+1) + 3 * (pKPFi->mPrevKPF->mnId) + 3);
                        
                        g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKPFi->mnId);
                        g2o::HyperGraph::Vertex *VV2 = optimizer.vertex((maxKPFid+1) + 3 * (pKPFi->mnId) + 1);
                        g2o::HyperGraph::Vertex *VG2 = optimizer.vertex((maxKPFid+1) + 3 * (pKPFi->mnId) + 2);
                        g2o::HyperGraph::Vertex *VA2 = optimizer.vertex((maxKPFid+1) + 3 * (pKPFi->mnId) + 3);

                        if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2)
                        {
                              // Problem11 Error0, 0, 0, 0, 0x7fffa8043050, 0x7fffa8043460 : 第一帧pKPFi的mPrevKPF并没有添加为vertex
                              cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << endl;
                              continue;
                        }

                        // 设置边
                        EdgeInertial *ei = new EdgeInertial(pKPFi->mpImuPreintegrated);
                        ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
                        ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
                        ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));
                        ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
                        ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
                        ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));

                        g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
                        ei->setRobustKernel(rki);
                        // 9个自由度的卡方检验（0.05）
                        rki->setDelta(sqrt(16.92));

                        optimizer.addEdge(ei);
                        vei[i+1] = ei;

                        vegr[i+1] = new EdgeGyroRW();
                        vegr[i+1]->setVertex(0, VG1);
                        vegr[i+1]->setVertex(1, VG2);
                        Eigen::Matrix3d InfoG = pKPFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast<double>().inverse();
                        vegr[i+1]->setInformation((1.0e-15)*InfoG);//1.0e-15 1.0e-19
                        //optimizer.addEdge(vegr[i+1]);

                        vear[i+1] = new EdgeAccRW();
                        vear[i+1]->setVertex(0, VA1);
                        vear[i+1]->setVertex(1, VA2);
                        Eigen::Matrix3d InfoA = pKPFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast<double>().inverse();
                        vear[i+1]->setInformation((1.0e-18)*InfoA);//1.0e-18 1.0e-22
                        //optimizer.addEdge(vear[i+1]); 

                        // PF
                        // Problem12 static_cast段错误？？
                        if(i == 0)
                        {
                              // IMU预积分约束：两帧之间位姿的变化量与IMU预积分的值偏差尽可能小
                              EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegrated);

                              // 根据上一帧的偏置设定当前帧的新偏置
                              pFrame->mpImuPreintegrated->SetNewBias(pKPFi->GetImuBias());
                              // 提取节点
                              g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(maxKPFid);
                              g2o::HyperGraph::Vertex *VV1 = optimizer.vertex((maxKPFid+1) + 3 * (maxKPFid) + 1);
                              g2o::HyperGraph::Vertex *VG1 = optimizer.vertex((maxKPFid+1) + 3 * (maxKPFid) + 2);
                              g2o::HyperGraph::Vertex *VA1 = optimizer.vertex((maxKPFid+1) + 3 * (maxKPFid) + 3);
                              
                              g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(maxKPFid+1);
                              g2o::HyperGraph::Vertex *VV2 = optimizer.vertex((maxKPFid+1) + 3 * (maxKPFid+1) + 1);
                              g2o::HyperGraph::Vertex *VG2 = optimizer.vertex((maxKPFid+1) + 3 * (maxKPFid+1) + 2);
                              g2o::HyperGraph::Vertex *VA2 = optimizer.vertex((maxKPFid+1) + 3 * (maxKPFid+1) + 3);

                              if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2)
                              {
                                    cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << endl;
                                    continue;
                              }

                              currentPFid = maxKPFid+1;

                              // 将上一关键帧四个顶点（P、V、BG、BA）和当前普通帧两个顶点（P、V）加入边
                              ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
                              ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
                              ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));
                              ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
                              ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
                              ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));

                              g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
                              ei->setRobustKernel(rki);
                              // 9个自由度的卡方检验（0.05）
                              rki->setDelta(sqrt(16.92));

                              // 边加入优化器
                              optimizer.addEdge(ei);
                              vei[i] = ei;

                              vegr[i] = new EdgeGyroRW();
                              vegr[i]->setVertex(0, VG1);
                              vegr[i]->setVertex(1, VG2);
                              Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3, 3>(9, 9).cast<double>().inverse();
                              vegr[i]->setInformation((1,0e-15)*InfoG);//1.0e-16
                              //optimizer.addEdge(vegr[i]);

                              vear[i] = new EdgeAccRW();
                              vear[i]->setVertex(0, VA1);
                              vear[i]->setVertex(1, VA2);
                              Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3, 3>(12, 12).cast<double>().inverse();
                              vear[i]->setInformation((1.0e-18)*InfoA);//1.0e-19
                              //optimizer.addEdge(vear[i]); 
                        }
                  }
            }
            else  
                  std::cout << pKPFi->mnId << " or " << pKPFi->mPrevKPF->mnId << " no imu" << std::endl;
      }

      // Step 5.优化
      optimizer.initializeOptimization();
      optimizer.computeActiveErrors();

      float err = optimizer.activeRobustChi2();
      optimizer.optimize(opt_it); // Originally to 2
      float err_end = optimizer.activeRobustChi2();

      // Step 6.取出优化结果，对应值赋值。
      VertexPose *VP2 = static_cast<VertexPose *>(optimizer.vertex(currentPFid));
      VertexVelocity *VV2 = static_cast<VertexVelocity *>(optimizer.vertex((maxKPFid+1) + 3 * currentPFid + 1));
      VertexGyroBias *VG2 = static_cast<VertexGyroBias *>(optimizer.vertex((maxKPFid+1) + 3 * currentPFid + 2));
      VertexAccBias *VA2 = static_cast<VertexAccBias *>(optimizer.vertex((maxKPFid+1) + 3 * currentPFid + 3));
      
      // 给当前帧设置优化后的旋转、位移、速度，用来更新位姿
      pFrame->SetImuPoseVelocity(VP2->estimate().Rwb.cast<float>(), VP2->estimate().twb.cast<float>(), VV2->estimate().cast<float>());

      // 给当前帧设置优化后的bg，ba
      Vector6d b;
      b << VG2->estimate(), VA2->estimate();
      pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]);

      return 1;
}

/**
 * @brief PoseInertialOptimizationLastFrame 中使用 Marginalize(H, 0, 14);
 * 使用舒尔补的方式边缘化海森矩阵，边缘化。
 * Jgr:相对重力的残差;Jg:重力方向的残差;Ja:加速度计测量值的雅克比矩阵;Jar:陀螺仪测量值的雅克比矩阵
 * 列数 6            3                    3                            3                         6           3             3              3
 * --------------------------------------------------------------------------------------------------------------------------------------------------- 行数
 * |  Jp1.t * Jp1  Jp1.t * Jv1         Jp1.t * Jg1                 Jp1.t * Ja1            |  Jp1.t * Jp2  Jp1.t * Jv2        0              0        |  6
 * |  Jv1.t * Jp1  Jv1.t * Jv1         Jv1.t * Jg1                 Jv1.t * Ja1            |  Jv1.t * Jp2  Jv1.t * Jv2        0              0        |  3
 * |  Jg1.t * Jp1  Jg1.t * Jv1  Jg1.t * Jg1 + Jgr1.t * Jgr1        Jg1.t * Ja1            |  Jg1.t * Jp2  Jg1.t * Jv2  Jgr1.t * Jgr2        0        |  3
 * |  Ja1.t * Jp1  Ja1.t * Jv1         Ja1.t * Jg1           Ja1.t * Ja1 + Jar1.t * Jar1  |  Ja1.t * Jp2  Ja1.t * Jv2  Jar1.t * Jar2        0        |  3
 * |--------------------------------------------------------------------------------------------------------------------------------------------------
 * |  Jp2.t * Jp1  Jp2.t * Jv1         Jp2.t * Jg1                 Jp2.t * Ja1            |  Jp2.t * Jp2  Jp2.t * Jv2        0              0        |  6
 * |  Jv2.t * Jp1  Jv2.t * Jv1         Jv2.t * Jg1                 Jv2.t * Ja1            |  Jv2.t * Jp2  Jv2.t * Jv2        0              0        |  3
 * |      0            0              Jgr2.t * Jgr1                      0                |        0           0       Jgr2.t * Jgr2        0        |  3
 * |      0            0                    0                     Jar2.t * Jar1           |        0           0             0        Jar2.t * Jar2  |  3
 * ---------------------------------------------------------------------------------------------------------------------------------------------------
 * @param H 30*30的海森矩阵
 * @param start 开始位置
 * @param end 结束位置
 */
Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end)
{
      // Goal
      // a  | ab | ac       a*  | 0 | ac*
      // ba | b  | bc  -->  0   | 0 | 0
      // ca | cb | c        ca* | 0 | c*

      // Size of block before block to marginalize
      const int a = start;
      // Size of block to marginalize
      const int b = end - start + 1;
      // Size of block after block to marginalize
      const int c = H.cols() - (end + 1);

      // Reorder as follows:
      // a  | ab | ac       a  | ac | ab
      // ba | b  | bc  -->  ca | c  | cb
      // ca | cb | c        ba | bc | b

      Eigen::MatrixXd Hn = Eigen::MatrixXd::Zero(H.rows(), H.cols());
      if (a > 0)
      {
            Hn.block(0, 0, a, a) = H.block(0, 0, a, a);
            Hn.block(0, a + c, a, b) = H.block(0, a, a, b);
            Hn.block(a + c, 0, b, a) = H.block(a, 0, b, a);
      }
      if (a > 0 && c > 0)
      {
            Hn.block(0, a, a, c) = H.block(0, a + b, a, c);
            Hn.block(a, 0, c, a) = H.block(a + b, 0, c, a);
      }
      if (c > 0)
      {
            Hn.block(a, a, c, c) = H.block(a + b, a + b, c, c);
            Hn.block(a, a + c, c, b) = H.block(a + b, a, c, b);
            Hn.block(a + c, a, b, c) = H.block(a, a + b, b, c);
      }
      Hn.block(a + c, a + c, b, b) = H.block(a, a, b, b);

      // Perform marginalization (Schur complement)
      Eigen::JacobiSVD<Eigen::MatrixXd> svd(Hn.block(a + c, a + c, b, b), Eigen::ComputeThinU | Eigen::ComputeThinV);
      Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType singularValues_inv = svd.singularValues();
      for (int i = 0; i < b; ++i)
      {
            if (singularValues_inv(i) > 1e-6)
                  singularValues_inv(i) = 1.0 / singularValues_inv(i);
            else
                  singularValues_inv(i) = 0;
      }
      Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose();
      Hn.block(0, 0, a + c, a + c) = Hn.block(0, 0, a + c, a + c) - Hn.block(0, a + c, a + c, b) * invHb * Hn.block(a + c, 0, b, a + c);
      Hn.block(a + c, a + c, b, b) = Eigen::MatrixXd::Zero(b, b);
      Hn.block(0, a + c, a + c, b) = Eigen::MatrixXd::Zero(a + c, b);
      Hn.block(a + c, 0, b, a + c) = Eigen::MatrixXd::Zero(b, a + c);

      // Inverse reorder
      // a*  | ac* | 0       a*  | 0 | ac*
      // ca* | c*  | 0  -->  0   | 0 | 0
      // 0   | 0   | 0       ca* | 0 | c*
      Eigen::MatrixXd res = Eigen::MatrixXd::Zero(H.rows(), H.cols());
      if (a > 0)
      {
            res.block(0, 0, a, a) = Hn.block(0, 0, a, a);
            res.block(0, a, a, b) = Hn.block(0, a + c, a, b);
            res.block(a, 0, b, a) = Hn.block(a + c, 0, b, a);
      }
      if (a > 0 && c > 0)
      {
            res.block(0, a + b, a, c) = Hn.block(0, a, a, c);
            res.block(a + b, 0, c, a) = Hn.block(a, 0, c, a);
      }
      if (c > 0)
      {
            res.block(a + b, a + b, c, c) = Hn.block(a, a, c, c);
            res.block(a + b, a, c, b) = Hn.block(a, a + c, c, b);
            res.block(a, a + b, b, c) = Hn.block(a + c, a, b, c);
      }

      res.block(a, a, b, b) = Hn.block(a + c, a + c, b, b);

      return res;
}
/**************************************以下为局部优化**************************************************************/

/**
 * @brief 局部地图＋惯导BA LocalOptimizing中使用，地图经过imu初始化后用这个函数
 *
 * @param[in] pKPF               //关键帧
 * @param[in] pbStopFlag        //是否停止的标志
 * @param[in] pMap              //地图
 * @param[in] num_fixedKPF       //固定不优化关键帧的数目
 * @param[in] num_OptKPF
 * @param[in] num_MPs
 * @param[in] num_edges
 * @param[in] bRecInit          经过BA2初始化后这个变量为false
 */
void Optimizer::LocalInertialBA(
    KeyPoseFrame *pKPF, bool *pbStopFlag, Map *pMap, int &num_fixedKPF, int &num_OptKPF,
    int &num_MPs, int &num_edges, bool bRecInit)
{
      // Step 1. 确定待优化的关键帧们
      Map *pCurrentMap = pKPF->GetMap();

      int maxOpt = 50; // 最大优化关键帧数目
      int opt_it = 5; // 每次优化的迭代次数 4

      // 预计待优化的关键帧数，min函数是为了控制优化关键帧的数量
      const int Nd = std::min((int)pCurrentMap->KeyPoseFramesInMap() - 2, maxOpt);
      const unsigned long maxKPFid = pKPF->mnId;

      vector<KeyPoseFrame *> vpOptimizableKPFs;
      const vector<KeyPoseFrame *> vpNeighsKPFs = pKPF->GetVectorCovisibleKeyPoseFrames();
      list<KeyPoseFrame *> lpOptVisKFs;

      vpOptimizableKPFs.reserve(Nd);
      vpOptimizableKPFs.push_back(pKPF);
      pKPF->mnBALocalForKPF = pKPF->mnId;

      for (int i = 1; i < Nd; i++)
      {
            if (vpOptimizableKPFs.back()->mPrevKPF)
            {
                  vpOptimizableKPFs.push_back(vpOptimizableKPFs.back()->mPrevKPF);
                  vpOptimizableKPFs.back()->mnBALocalForKPF = pKPF->mnId;
            }
            else
                  break;
      }

      int N = vpOptimizableKPFs.size();

      // Step 2. 固定一帧，为vpOptimizableKPFs中最早的那一关键帧的上一关键帧，如果没有上一关键帧了就用最早的那一帧
      list<KeyPoseFrame *> lFixedKeyPoseFrames;
      if (vpOptimizableKPFs.back()->mPrevKPF)
      {
            lFixedKeyPoseFrames.push_back(vpOptimizableKPFs.back()->mPrevKPF);
            vpOptimizableKPFs.back()->mPrevKPF->mnBAFixedForKPF = pKPF->mnId;
      }
      else
      {
            vpOptimizableKPFs.back()->mnBALocalForKPF = 0;
            vpOptimizableKPFs.back()->mnBAFixedForKPF = pKPF->mnId;
            lFixedKeyPoseFrames.push_back(vpOptimizableKPFs.back());
            vpOptimizableKPFs.pop_back();
      }

      bool bNonFixed = (lFixedKeyPoseFrames.size() == 0);

      // Step 3. 构造优化器，正式开始优化
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolverX::LinearSolverType *linearSolver;
      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();

      g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);

      g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      // 在优化问题中，Lambda通常被称为Levenberg-Marquardt算法的阻尼因子，它用于平衡两种不同的优化策略：一种是梯度下降法（Gradient Descent），另一种是高斯牛顿法（Gauss-Newton）。
      // 通过调整Lambda的值，可以在这两种策略之间进行权衡，以加速或稳定优化过程。
      solver->setUserLambdaInit(1e-2); // to avoid iterating for finding optimal lambda
      optimizer.setAlgorithm(solver);

      // Step 4. 建立关于关键帧的节点，其中包括，位姿，速度，以及两个偏置
      N = vpOptimizableKPFs.size();
      for (int i = 0; i < N; i++)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i];

            VertexPose *VP = new VertexPose(pKPFi);
            VP->setId(pKPFi->mnId);
            VP->setFixed(false);
            optimizer.addVertex(VP);

            if (pKPFi->bImu)
            {
                  VertexVelocity *VV = new VertexVelocity(pKPFi);
                  VV->setId(maxKPFid + 3 * (pKPFi->mnId) + 1);
                  VV->setFixed(false);
                  optimizer.addVertex(VV);
                  VertexGyroBias *VG = new VertexGyroBias(pKPFi);
                  VG->setId(maxKPFid + 3 * (pKPFi->mnId) + 2);
                  VG->setFixed(false);
                  optimizer.addVertex(VG);
                  VertexAccBias *VA = new VertexAccBias(pKPFi);
                  VA->setId(maxKPFid + 3 * (pKPFi->mnId) + 3);
                  VA->setFixed(false);
                  optimizer.addVertex(VA);
            }
      }

      // Step 5. 建立关于固定关键帧的节点，其中包括，位姿，速度，以及两个偏置
      for (list<KeyPoseFrame *>::iterator lit = lFixedKeyPoseFrames.begin(), lend = lFixedKeyPoseFrames.end(); lit != lend; lit++)
      {
            KeyPoseFrame *pKPFi = *lit;
            VertexPose *VP = new VertexPose(pKPFi);
            VP->setId(pKPFi->mnId);
            VP->setFixed(true);
            optimizer.addVertex(VP);

            if (pKPFi->bImu) // This should be done only for keyposeframe just before temporal window
            {
                  VertexVelocity *VV = new VertexVelocity(pKPFi);
                  VV->setId(maxKPFid + 3 * (pKPFi->mnId) + 1);
                  VV->setFixed(true);
                  optimizer.addVertex(VV);
                  VertexGyroBias *VG = new VertexGyroBias(pKPFi);
                  VG->setId(maxKPFid + 3 * (pKPFi->mnId) + 2);
                  VG->setFixed(true);
                  optimizer.addVertex(VG);
                  VertexAccBias *VA = new VertexAccBias(pKPFi);
                  VA->setId(maxKPFid + 3 * (pKPFi->mnId) + 3);
                  VA->setFixed(true);
                  optimizer.addVertex(VA);
            }
      }

      vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);
      vector<EdgeGyroRW *> vegr(N, (EdgeGyroRW *)NULL);
      vector<EdgeAccRW *> vear(N, (EdgeAccRW *)NULL);
      // Step 6. 建立惯性边，没有imu跳过
      for (int i = 0; i < N; i++)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i];

            if (!pKPFi->mPrevKPF)
            {
                  cout << "Optimizer : LocalInertialBA : NOT INERTIAL LINK TO PREVIOUS FRAME!!!!" << endl;
                  continue;
            }
            if (pKPFi->bImu && pKPFi->mPrevKPF->bImu && pKPFi->mpImuPreintegrated)
            {
                  pKPFi->mpImuPreintegrated->SetNewBias(pKPFi->mPrevKPF->GetImuBias());
                  g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKPFi->mPrevKPF->mnId);
                  g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mPrevKPF->mnId) + 1);
                  g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mPrevKPF->mnId) + 2);
                  g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mPrevKPF->mnId) + 3);
                  g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKPFi->mnId);
                  g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 1);
                  g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 2);
                  g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 3);

                  if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2)
                  {
                        cerr << "Error " << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl;
                        continue;
                  }

                  vei[i] = new EdgeInertial(pKPFi->mpImuPreintegrated);

                  vei[i]->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
                  vei[i]->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
                  vei[i]->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));
                  vei[i]->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
                  vei[i]->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
                  vei[i]->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));

                  // 到最早的一个可优化的关键帧或者地图没有ba2时添加鲁棒核函数
                  if (i == N - 1 || bRecInit)
                  {
                        // 所有惯性残差都没有鲁棒核，但不包括窗口内最早一个可优化关键帧与第一个固定关键帧链接起来的惯性残差。
                        // 该度量的信息矩阵也被降权。这样做是为了避免由于固定变量而累积错误。
                        g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
                        vei[i]->setRobustKernel(rki);

                        if (i == N - 1)
                              // 信息矩阵，即协方差矩阵的逆。信息矩阵反映了这个边的可信度，越大表示越可信，越小表示越不可信。
                              // 将信息矩阵乘以一个很小的系数降低这个边的可信度。因为窗口内最早一个可优化关键帧与第一个固定关键帧链接起来的惯性残差可能不准确。
                              // 通过降低信息矩阵，可以减少这个边对优化结果的影响，使得优化更加稳定和准确。
                              vei[i]->setInformation(vei[i]->information() * 1e-2);
                        
                        // 为鲁棒核函数设置参数delta，即异常值检测的阈值     
                        // 如果delta设置得太大，那么鲁棒核函数就会接近于平方误差函数，对异常值的惩罚太小，优化结果可能不准确。
                        // 如果delta设置得太小，那么鲁棒核函数就会接近于线性误差函数，对正常值的惩罚太大，优化结果可能不稳定。
                        // 也许要大一点。。 
                        rki->setDelta(sqrt(16.92)); 
                  }
                  optimizer.addEdge(vei[i]);

                  
                  vegr[i] = new EdgeGyroRW();
                  vegr[i]->setVertex(0, VG1);
                  vegr[i]->setVertex(1, VG2);
                  Eigen::Matrix3d InfoG = pKPFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast<double>().inverse();
                  // Problem17: 第二次漂移
                  vegr[i]->setInformation((1.0e-19)*InfoG);//param 纯后端优化 1.0e-19 21
                  //vegr[i]->setInformation(InfoG);
                  optimizer.addEdge(vegr[i]);

                  vear[i] = new EdgeAccRW();
                  vear[i]->setVertex(0, VA1);
                  vear[i]->setVertex(1, VA2);
                  Eigen::Matrix3d InfoA = pKPFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast<double>().inverse();
                  vear[i]->setInformation((1.0e-20)*InfoA);//param 纯后端优化 1.0e-22 24
                  //vear[i]->setInformation(InfoA);
                  optimizer.addEdge(vear[i]);
                  
            }
            else
                  cout << "Optimizer : LocalInertialBA : ERROR building inertial edge" << endl;
      }

      // Problem14.5 打印优化前的关键帧的p和v
      std::cout << "Optimizer : LocalInertialBA Start" <<std::endl;
      /*
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i];
            // 获取p
            Eigen::Vector3d twb = pKPFi->GetImuPosition().cast<double>();
            double px = twb[0];
            double py = twb[1];
            double pz = twb[2];

            // 获取v
            Eigen::Vector3d Vw = pKPFi->GetVelocity().cast<double>();
            double vx = Vw[0];
            double vy = Vw[1];
            double vz = Vw[2];

            // 打印
            std::cout << "Frame" << i << " :p[" << px << "," << py << "," << pz << "],v[" << vx << "," << vy << "," << vz << "]" << std::endl;
      }
      */
      // Step 7. 开始优化initializeOptimization
      optimizer.initializeOptimization(); // 成员函数，用于初始化优化器，包括设置线性求解器，边缘化策略，优化级别等
      // 成员函数，用于计算当前优化问题中所有激活边（active edges）的误差，并更新误差向量和雅可比矩阵
      // 每次优化迭代之前调用的，用于计算当前的残差和梯度，为优化算法提供必要的信息
      optimizer.computeActiveErrors(); 

      // 成员函数，用于计算当前优化问题中所有激活边（active edges）的鲁棒卡方值（robust chi-square），即加权误差平方和
      // 可以反映优化问题的拟合程度，越小表示越接近最优解，越大表示越偏离最优解。这个值可以用于评估优化的效果，或者作为停止优化的条件之一
      float err = optimizer.activeRobustChi2(); 
      optimizer.optimize(opt_it); // 执行指定次数（opt_it）的优化迭代，并返回实际执行的迭代次数
      float err_end = optimizer.activeRobustChi2();
      if (pbStopFlag)
            optimizer.setForceStopFlag(pbStopFlag);

      // Get Map Mutex and erase outliers
      unique_lock<mutex> lock(pMap->mMutexMapUpdate);

      // TODO: Some convergence problems have been detected here
      // if (2 * err < err_end || isnan(err) || isnan(err_end)) // bGN)
      // {
      //       cout << "FAIL LOCAL-INERTIAL BA!!!!" << endl;
      //       return;
      // }

      for (list<KeyPoseFrame *>::iterator lit = lFixedKeyPoseFrames.begin(), lend = lFixedKeyPoseFrames.end(); lit != lend; lit++)
            (*lit)->mnBAFixedForKPF = 0;

      // Step 8. 取出结果
      // Local Keyframes
      N = vpOptimizableKPFs.size();
      for (int i = 0; i < N; i++)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i];

            VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKPFi->mnId));
            Sophus::SE3f Tow(VP->estimate().Row[0].cast<float>(), VP->estimate().tow[0].cast<float>());
            pKPFi->SetPose(Tow);
            pKPFi->mnBALocalForKPF = 0;

            if (pKPFi->bImu)
            {
                  VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 1));
                  pKPFi->SetVelocity(VV->estimate().cast<float>());
                  VertexGyroBias *VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 2));
                  VertexAccBias *VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 3));
                  Vector6d b;
                  b << VG->estimate(), VA->estimate();
                  pKPFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));
            }
      }

      // Problem14.5 打印优化后的关键帧的p和v
      /*
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpOptimizableKPFs[i];
            // 获取p
            Eigen::Vector3d twb = pKPFi->GetImuPosition().cast<double>();
            double px = twb[0];
            double py = twb[1];
            double pz = twb[2];

            // 获取v
            Eigen::Vector3d Vw = pKPFi->GetVelocity().cast<double>();
            double vx = Vw[0];
            double vy = Vw[1];
            double vz = Vw[2];

            // 打印
            std::cout << "Frame" << i << " :p[" << px << "," << py << "," << pz << "],v[" << vx << "," << vy << "," << vz << "]" << std::endl;
      }
      */
      std::cout << "Optimizer : LocalInertialBA END" <<std::endl;

       pMap->IncreaseChangeIndex();
}

/**************************************以下为IMU全局优化**************************************************************/

/**
 * @brief imu初始化优化，LocalOptimizing::InitializeIMU中使用
 * @param pMap 地图
 * @param its 迭代次数
 * @param bFixLocal 是否固定局部，为false
 * @param nLoopId 回环id
 * @param pbStopFlag 是否停止的标志
 * @param bInit 提供priorG时为true，此时偏置只优化最后一帧的至0，然后所有关键帧的偏置都赋值为优化后的值
 *              若为false，则建立每两帧之间的偏置边，优化使其相差为0
 * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用
 * @param priorA 加速度计偏置的信息矩阵系数
 * @param vSingVal 没用，估计调试用的
 * @param bHess 没用，估计调试用的
 */
void Optimizer::FullInertialBA(
    Map *pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool *pbStopFlag,
    bool bInit, float priorG, float priorA, Eigen::VectorXd *vSingVal, bool *bHess)
{
      // 获取地图里所有kpf，以及最大kpf的id
      long unsigned int maxKPFid = pMap->GetMaxKPFid();
      vector<KeyPoseFrame *> vpKPF = pMap->GetAllKeyPoseFrames(); //old->new

      vector<KeyPoseFrame *> vpKPFs;
      int maxOpt = 35; //最大优化关键帧数目

      //从vpKPF中获取最多30个关键帧到vpKPFs中
      //首先，判断vpKPF的大小是否小于等于maxOpt，如果是，就直接把vpKPF赋值给vpKPFs
      if (vpKPF.size() <= maxOpt) 
      {
            vpKPFs = vpKPF;
      }
      else
      {
            //否则，需要从vpKPF的尾部开始，取出最多maxOpt个关键帧指针，放入vpKPFs中
            for (int i = vpKPF.size() - 1; i >= vpKPF.size() - maxOpt; i--)
            {
                  // 从vpKPF的尾部取出一个关键帧指针
                  KeyPoseFrame *pKPF = vpKPF[i];
                  // 把关键帧指针放入vpKPFs的尾部
                  vpKPFs.push_back(pKPF);
            }
      }

      // Setup optimizer
      // 1. 很经典的一套设置优化器流程
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolverX::LinearSolverType *linearSolver;

      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();

      g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);

      g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      solver->setUserLambdaInit(1e-5);
      optimizer.setAlgorithm(solver);
      optimizer.setVerbose(false);

      if (pbStopFlag)
            optimizer.setForceStopFlag(pbStopFlag);

      int nNonFixed = 0;

      // 2. 设置关键帧与偏置节点
      // Set KeyFrame vertices
      KeyPoseFrame *pIncKPF;  // vpKPFs中最后一个id符合要求的关键帧
      for (size_t i = 0; i < vpKPFs.size(); i++)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            if (pKPFi->mnId > maxKPFid)
                  continue;
            VertexPose *VP = new VertexPose(pKPFi);
            VP->setId(pKPFi->mnId);
            pIncKPF = pKPFi;
            bool bFixed = false;
            if (bFixLocal)
            {
                  bFixed = (pKPFi->mnBALocalForKPF >= (maxKPFid - 1)) || (pKPFi->mnBAFixedForKPF >= (maxKPFid - 1));
                  if (!bFixed)
                        nNonFixed++;
                  VP->setFixed(bFixed);  // 固定，这里可能跟回环有关
            }
            optimizer.addVertex(VP);
            // 如果是初始化的那几个及后面的关键帧，加入速度节点
            if (pKPFi->bImu)
            {
                  VertexVelocity *VV = new VertexVelocity(pKPFi);
                  VV->setId(maxKPFid + 3 * (pKPFi->mnId) + 1);
                  VV->setFixed(bFixed);
                  optimizer.addVertex(VV);
                  // priorA==0.f 时 bInit为false，也就是又加入了偏置节点
                  if (!bInit)
                  {
                        VertexGyroBias *VG = new VertexGyroBias(pKPFi);
                        VG->setId(maxKPFid + 3 * (pKPFi->mnId) + 2);
                        VG->setFixed(bFixed);
                        optimizer.addVertex(VG);
                        VertexAccBias *VA = new VertexAccBias(pKPFi);
                        VA->setId(maxKPFid + 3 * (pKPFi->mnId) + 3);
                        VA->setFixed(bFixed);
                        optimizer.addVertex(VA);
                  }
            }
      }
      // priorA!=0.f 时 bInit为true，加入了偏置节点
      if (bInit)
      {
            VertexGyroBias *VG = new VertexGyroBias(pIncKPF);
            VG->setId(4 * maxKPFid + 2);
            VG->setFixed(false);
            optimizer.addVertex(VG);
            VertexAccBias *VA = new VertexAccBias(pIncKPF);
            VA->setId(4 * maxKPFid + 3);
            VA->setFixed(false);
            optimizer.addVertex(VA);
      }
      // false
      if (bFixLocal)
      {
            if (nNonFixed < 3)
                  return;
      }

      const int N = vpKPFs.size();
      vector<EdgeInertial *> vei(N, (EdgeInertial *)NULL);
      // 3. 添加关于imu的边
      // IMU links
      for (size_t i = 0; i < vpKPFs.size()-1; i++)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            // 必须有对应的上一个关键帧，感觉跟下面的判定冲突了
            if (!pKPFi->mPrevKPF)
            {
                  std::cout << "Optimizer::FullInertialBA : NOT INERTIAL LINK TO PREVIOUS FRAME!" << std::endl;
                  continue;
            }

            if (pKPFi->mPrevKPF && pKPFi->mnId <= maxKPFid)
            {
                  if (pKPFi->isBad() || pKPFi->mPrevKPF->mnId > maxKPFid)
                        continue;
                  // 这两个都必须为初始化后的关键帧
                  if (pKPFi->bImu && pKPFi->mPrevKPF->bImu)
                  {
                        // 3.1 根据上一帧的偏置设定当前帧的新偏置
                        pKPFi->mpImuPreintegrated->SetNewBias(pKPFi->mPrevKPF->GetImuBias());
                        // 3.2 提取节点
                        g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKPFi->mPrevKPF->mnId);
                        g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mPrevKPF->mnId) + 1);

                        g2o::HyperGraph::Vertex *VG1;
                        g2o::HyperGraph::Vertex *VA1;
                        g2o::HyperGraph::Vertex *VG2;
                        g2o::HyperGraph::Vertex *VA2;
                        // 根据不同输入配置相应的偏置节点
                        if (!bInit)
                        {
                              VG1 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mPrevKPF->mnId) + 2);
                              VA1 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mPrevKPF->mnId) + 3);
                              VG2 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 2);
                              VA2 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 3);
                        }
                        else
                        {
                              VG1 = optimizer.vertex(4 * maxKPFid + 2);
                              VA1 = optimizer.vertex(4 * maxKPFid + 3);
                        }

                        g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKPFi->mnId);
                        g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 1);

                        if (!bInit)
                        {
                              if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2)
                              {
                                    cout << "Optimizer::FullInertialBA : " <<"Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl;
                                    continue;
                              }
                        }
                        else
                        {
                              if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2)
                              {
                                    cout << "Optimizer::FullInertialBA : " << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << endl;
                                    continue;
                              }
                        }
                        // 3.3 设置边
                        EdgeInertial *ei = new EdgeInertial(pKPFi->mpImuPreintegrated);
                        ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
                        ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
                        ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));
                        ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
                        ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
                        ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));

                        g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
                        ei->setRobustKernel(rki);
                        // 9个自由度的卡方检验（0.05）
                        rki->setDelta(sqrt(16.92));

                        optimizer.addEdge(ei);
                        vei[i] = ei;
                        // 加了每一个关键帧的偏置时，还要优化两帧之间偏置的误差
                        if (!bInit)
                        {
                              EdgeGyroRW *egr = new EdgeGyroRW();
                              egr->setVertex(0, VG1);
                              egr->setVertex(1, VG2);
                              Eigen::Matrix3d InfoG = pKPFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast<double>().inverse();
                              egr->setInformation(InfoG);
                              egr->computeError();
                              optimizer.addEdge(egr);

                              EdgeAccRW *ear = new EdgeAccRW();
                              ear->setVertex(0, VA1);
                              ear->setVertex(1, VA2);
                              Eigen::Matrix3d InfoA = pKPFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast<double>().inverse();
                              ear->setInformation(InfoA);
                              ear->computeError();
                              optimizer.addEdge(ear);
                        }
                  }
                  else
                        cout << pKPFi->mnId << " or " << pKPFi->mPrevKPF->mnId << " no imu" << endl;
            }
      }

      // 只加入pIncKF帧的偏置，优化偏置到0
      if (bInit)
      {
            // Problem16.2 优化漂移，去除bias先验约束
            
            g2o::HyperGraph::Vertex *VG = optimizer.vertex(4 * maxKPFid + 2);
            g2o::HyperGraph::Vertex *VA = optimizer.vertex(4 * maxKPFid + 3);

            // Add prior to comon biases
            Eigen::Vector3f bprior;
            bprior.setZero();

            EdgePriorAcc *epa = new EdgePriorAcc(bprior);
            epa->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
            double infoPriorA = priorA; 
            epa->setInformation((1.0e-25)*infoPriorA * Eigen::Matrix3d::Identity());//param 1.0e-22
            optimizer.addEdge(epa);

            EdgePriorGyro *epg = new EdgePriorGyro(bprior);
            epg->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
            double infoPriorG = priorG; 
            epg->setInformation((1.0e-22)*infoPriorG * Eigen::Matrix3d::Identity());//param 1.0e-19
            optimizer.addEdge(epg);
            
      }

      if (pbStopFlag)
            if (*pbStopFlag)
                  return;

      // Problem14.4 打印优化前的关键帧的p和v
      std::cout << "Optimizer::FullInertialBA Start" <<std::endl;
      /*
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            // 获取p
            Eigen::Vector3d twb = pKPFi->GetImuPosition().cast<double>();
            double px = twb[0];
            double py = twb[1];
            double pz = twb[2];

            // 获取v
            Eigen::Vector3d Vw = pKPFi->GetVelocity().cast<double>();
            double vx = Vw[0];
            double vy = Vw[1];
            double vz = Vw[2];

            // 打印
            std::cout << "Frame" << i << " :p[" << px << "," << py << "," << pz << "],v[" << vx << "," << vy << "," << vz << "]" << std::endl;
      }
      */
      optimizer.initializeOptimization();
      optimizer.computeActiveErrors(); 

      // 成员函数，用于计算当前优化问题中所有激活边（active edges）的鲁棒卡方值（robust chi-square），即加权误差平方和
      // 可以反映优化问题的拟合程度，越小表示越接近最优解，越大表示越偏离最优解。这个值可以用于评估优化的效果，或者作为停止优化的条件之一
      float err = optimizer.activeRobustChi2(); 
      optimizer.optimize(its); // 执行指定次数（opt_it）的优化迭代，并返回实际执行的迭代次数
      float err_end = optimizer.activeRobustChi2();

      // 5. 取出优化结果，对应的值赋值
      // Recover optimized data
      // Keyposeframes
      for (size_t i = 0; i < vpKPFs.size(); i++)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            if (pKPFi->mnId > maxKPFid)
                  continue;
            VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKPFi->mnId));
            // Problem15 不需要回环
            //if (nLoopId == 0)
            if(1)
            {
                  Sophus::SE3f Tow(VP->estimate().Row[0].cast<float>(), VP->estimate().tow[0].cast<float>());
                  pKPFi->SetPose(Tow);
            }
            else
            {
                  pKPFi->mTowGBA = Sophus::SE3f(VP->estimate().Row[0].cast<float>(), VP->estimate().tow[0].cast<float>());
                  pKPFi->mnBAGlobalForKPF = nLoopId;
            }
            if (pKPFi->bImu)
            {
                  VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 1));
                  //if (nLoopId == 0)
                  if(1)
                  {
                        pKPFi->SetVelocity(VV->estimate().cast<float>());
                  }
                  else
                  {
                        pKPFi->mVwbGBA = VV->estimate().cast<float>();
                  }

                  VertexGyroBias *VG;
                  VertexAccBias *VA;
                  if (!bInit)
                  {
                        VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 2));
                        VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKPFid + 3 * (pKPFi->mnId) + 3));
                  }
                  else
                  {
                        VG = static_cast<VertexGyroBias *>(optimizer.vertex(4 * maxKPFid + 2));
                        VA = static_cast<VertexAccBias *>(optimizer.vertex(4 * maxKPFid + 3));
                  }

                  Vector6d vb;
                  vb << VG->estimate(), VA->estimate();
                  IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]);
                  //if (nLoopId == 0)
                  if(1)
                  {
                        pKPFi->SetNewBias(b);
                  }
                  else
                  {
                        pKPFi->mBiasGBA = b;
                  }
            }
      }

      // Problem14.4 打印优化后的关键帧的p和v
      /*
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            // 获取p
            Eigen::Vector3d twb = pKPFi->GetImuPosition().cast<double>();
            double px = twb[0];
            double py = twb[1];
            double pz = twb[2];

            // 获取v
            Eigen::Vector3d Vw = pKPFi->GetVelocity().cast<double>();
            double vx = Vw[0];
            double vy = Vw[1];
            double vz = Vw[2];

            // 打印
            std::cout << "Frame" << i << " :p[" << px << "," << py << "," << pz << "],v[" << vx << "," << vy << "," << vz << "]" << std::endl;
      }
      */
      std::cout << "Optimizer::FullInertialBA END" <<std::endl;

      pMap->IncreaseChangeIndex();
}

/**************************************以下为尺度与重力优化**************************************************************/

/**
 * @brief imu初始化优化，LocalOptimizing::InitializeIMU中使用，其中kpf的位姿是固定不变的
 * 优化目标 重力方向，尺度，速度与偏置
 * @param pMap 地图
 * @param Rwg 重力方向到速度方向的转角
 * @param scale 尺度（输出cout用）
 * @param bg 陀螺仪偏置（输出cout用）
 * @param ba 加速度计偏置（输出cout用）
 * @param bMono 是否为单目
 * @param covInertial 惯导协方差矩阵(暂时没用，9*9的0矩阵)
 * @param bFixedVel 是否固定速度不优化，false
 * @param bGauss  没用，false
 * @param priorG 陀螺仪偏置的信息矩阵系数
 * @param priorA 加速度计偏置的信息矩阵系数
 */
void Optimizer::InertialOptimization(
    Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba,
    Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA)
{
      //std::cout << "Optimizer : inertial optimization" << std::endl;
      int its = 50;//200
      long unsigned int maxKPFid = pMap->GetMaxKPFid();
      const vector<KeyPoseFrame *> vpKPF = pMap->GetAllKeyPoseFrames();  // 获取所有关键帧

      vector<KeyPoseFrame *> vpKPFs;
      int maxOpt = 35; // 最大优化关键帧数目
      //从vpKPF中获取最多60个关键帧到vpKPFs中
      //首先，判断vpKPF的大小是否小于等于maxOpt，如果是，就直接把vpKPF赋值给vpKPFs
      if (vpKPF.size() <= maxOpt) 
      {
            vpKPFs = vpKPF;
      }
      else
      {
            //否则，需要从vpKPF的尾部开始，取出最多maxOpt个关键帧指针，放入vpKPFs中
            for (int i = vpKPF.size() - 1; i >= vpKPF.size() - maxOpt; i--)
            {
                  // 从vpKPF的尾部取出一个关键帧指针
                  KeyPoseFrame *pKPF = vpKPF[i];
                  // 把关键帧指针放入vpKPFs的尾部
                  vpKPFs.push_back(pKPF);
            }
      }

      // 1. 构建优化器
      // Problem9 无法return?? cmakelist缺少行add_subdirectory(Thirdparty/g2o)
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolverX::LinearSolverType *linearSolver;
      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();
      g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
      g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      
      if (priorG != 0.f)
            solver->setUserLambdaInit(1e3); // lambda值是Levenberg-Marquardt算法中的一个参数，它用于控制算法在高斯牛顿法和梯度下降法之间的平衡

      optimizer.setAlgorithm(solver);

      // 2. 确定关键帧节点（锁住的位姿及可优化的速度）
      for (size_t i = 0; i < vpKPFs.size(); i++)  
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            // 跳过id大于当前地图最大id的关键帧
            if (pKPFi->mnId > maxKPFid)
                  continue;
            VertexPose *VP = new VertexPose(pKPFi);  // 继承于public g2o::BaseVertex<6, ImuOdomPose>
            VP->setId(pKPFi->mnId);
            VP->setFixed(true);
            optimizer.addVertex(VP);

            VertexVelocity *VV = new VertexVelocity(pKPFi);  // 继承于public g2o::BaseVertex<3, Eigen::Vector3d>
            VV->setId(maxKPFid + (pKPFi->mnId) + 1);
            if (bFixedVel)
                  VV->setFixed(true);
            else
                  VV->setFixed(false);

            optimizer.addVertex(VV);
      }

      // 3. 确定偏置节点，陀螺仪与加速度计
      VertexGyroBias *VG = new VertexGyroBias(vpKPFs.front());
      VG->setId(maxKPFid * 2 + 2);
      if (bFixedVel)
            VG->setFixed(true);
      else
            VG->setFixed(false);
      optimizer.addVertex(VG);
      VertexAccBias *VA = new VertexAccBias(vpKPFs.front());
      VA->setId(maxKPFid * 2 + 3);
      if (bFixedVel)
            VA->setFixed(true);
      else
            VA->setFixed(false);

      optimizer.addVertex(VA);

      // prior acc bias
      Eigen::Vector3f bprior;
      bprior.setZero();

      // Problem16.1 优化漂移，去除bias先验约束
      // 4. 添加关于加速度计与陀螺仪偏置的边，这两个边加入是保证第一帧的偏置为0
      
      EdgePriorAcc *epa = new EdgePriorAcc(bprior);
      epa->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
      double infoPriorA = priorA;
      epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity());
      optimizer.addEdge(epa);
      EdgePriorGyro *epg = new EdgePriorGyro(bprior);
      epg->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
      double infoPriorG = priorG;
      epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity());
      optimizer.addEdge(epg);
      

      // 5. 添加关于重力方向与尺度的节点
      VertexGDir *VGDir = new VertexGDir(Rwg);
      VGDir->setId(maxKPFid * 2 + 4);
      VGDir->setFixed(false);
      optimizer.addVertex(VGDir);
      VertexScale *VS = new VertexScale(scale);
      VS->setId(maxKPFid * 2 + 5);
      VS->setFixed(true); // Fixed for stereo case
      optimizer.addVertex(VS);

      // 6. imu信息链接重力方向与尺度信息
      vector<EdgeInertialGS *> vpei;  // 后面虽然加入了边，但是没有用到，应该调试用的
      vpei.reserve(vpKPFs.size());
      vector<pair<KeyPoseFrame *, KeyPoseFrame *>> vppUsedKPF;
      vppUsedKPF.reserve(vpKPFs.size());  // 后面虽然加入了关键帧，但是没有用到，应该调试用的

      for (size_t i = 0; i < vpKPFs.size()-1; i++)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];

            if (pKPFi->mPrevKPF && pKPFi->mnId <= maxKPFid)
            {
                  if (pKPFi->isBad() || pKPFi->mPrevKPF->mnId > maxKPFid)
                        continue;
                  if (!pKPFi->mpImuPreintegrated)
                        std::cout << "Optimizer : inertial optimization : Not preintegrated measurement" << std::endl;
                  // 到这里的条件是pKFi是好的，并且它有上一个关键帧，且他们的id要小于最大id
                  // 6.1 检查节点指针是否为空
                  // 将pKFi偏置设定为上一关键帧的偏置
                  pKPFi->mpImuPreintegrated->SetNewBias(pKPFi->mPrevKPF->GetImuBias());
                  g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKPFi->mPrevKPF->mnId);
                  g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKPFid + (pKPFi->mPrevKPF->mnId) + 1);
                  g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKPFi->mnId);
                  g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKPFid + (pKPFi->mnId) + 1);
                  g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKPFid * 2 + 2);
                  g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKPFid * 2 + 3);
                  g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKPFid * 2 + 4);
                  g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKPFid * 2 + 5);
                  if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
                  {
                        cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl;

                        continue;
                  }
                  // 6.2 这是一个大边。。。。包含了上面所有信息，注意到前面的两个偏置也做了两个一元边加入
                  EdgeInertialGS *ei = new EdgeInertialGS(pKPFi->mpImuPreintegrated);
                  ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
                  ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
                  ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
                  ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
                  ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
                  ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
                  ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir));
                  ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS));

                  vpei.push_back(ei);

                  vppUsedKPF.push_back(make_pair(pKPFi->mPrevKPF, pKPFi));
                  optimizer.addEdge(ei);
            }
      }

      // Compute error for different scales
      std::set<g2o::HyperGraph::Edge *> setEdges = optimizer.edges();

      const int N = vpKPFs.size();
      // Problem14.3 打印优化前的关键帧的p和v
      std::cout << "Optimizer::InertialOptimization Start" <<std::endl;
      /*
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            // 获取p
            Eigen::Vector3d twb = pKPFi->GetImuPosition().cast<double>();
            double px = twb[0];
            double py = twb[1];
            double pz = twb[2];

            // 获取v
            Eigen::Vector3d Vw = pKPFi->GetVelocity().cast<double>();
            double vx = Vw[0];
            double vy = Vw[1];
            double vz = Vw[2];

            // 打印
            std::cout << "Frame" << i << " :p[" << px << "," << py << "," << pz << "],v[" << vx << "," << vy << "," << vz << "]" << std::endl;
      }
      */
      optimizer.setVerbose(false);
      optimizer.initializeOptimization();
      optimizer.computeActiveErrors(); 

      // 成员函数，用于计算当前优化问题中所有激活边（active edges）的鲁棒卡方值（robust chi-square），即加权误差平方和
      // 可以反映优化问题的拟合程度，越小表示越接近最优解，越大表示越偏离最优解。这个值可以用于评估优化的效果，或者作为停止优化的条件之一
      float err = optimizer.activeRobustChi2(); 
      optimizer.optimize(its); // 执行指定次数（opt_it）的优化迭代，并返回实际执行的迭代次数
      //std::this_thread::sleep_for(std::chrono::seconds(1));
      float err_end = optimizer.activeRobustChi2();

      // 7. 取数
      scale = VS->estimate();

      // Recover optimized data
      // Biases
      VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKPFid * 2 + 2));
      VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKPFid * 2 + 3));
      Vector6d vb;
      vb << VG->estimate(), VA->estimate();
      bg << VG->estimate();
      ba << VA->estimate();
      scale = VS->estimate();

      IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], 0.04*vb[2]);//param 0.04
      Rwg = VGDir->estimate().Rwg;

      // Keyframes velocities and biases
      
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            if (pKPFi->mnId > maxKPFid)
                  continue;

            VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKPFid + (pKPFi->mnId) + 1));
            Eigen::Vector3d Vw = VV->estimate(); // Velocity is scaled after
            pKPFi->SetVelocity(Vw.cast<float>());

            if ((pKPFi->GetGyroBias() - bg.cast<float>()).norm() > 0.01)
            {
                  // Problem18 角度偏移？？
                  pKPFi->SetNewBias(b);
                  if (pKPFi->mpImuPreintegrated)
                        pKPFi->mpImuPreintegrated->Reintegrate();
            }
            else
                  pKPFi->SetNewBias(b);
      }
      

      // Problem14.3 打印优化后的关键帧的p和v
      /*
      for (size_t i = 0; i < N; ++i)
      {
            KeyPoseFrame *pKPFi = vpKPFs[i];
            // 获取p
            Eigen::Vector3d twb = pKPFi->GetImuPosition().cast<double>();
            double px = twb[0];
            double py = twb[1];
            double pz = twb[2];

            // 获取v
            Eigen::Vector3d Vw = pKPFi->GetVelocity().cast<double>();
            double vx = Vw[0];
            double vy = Vw[1];
            double vz = Vw[2];

            // 打印
            std::cout << "Frame" << i << " :p[" << px << "," << py << "," << pz << "],v[" << vx << "," << vy << "," << vz << "]" << std::endl;
      }
      */
      std::cout << "Optimizer::InertialOptimization END" <<std::endl;
}

} //namespace msf


